﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using Microsoft.Kinect;
using Microsoft.Speech;
using Microsoft.Speech.AudioFormat;
using Microsoft.Speech.Recognition;
using Coding4Fun.Kinect.Wpf;



namespace FGP_Kpaint
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    /// 

    public partial class MainWindow : Window
    {

        double zaxis;
        private Boolean collision;
        private WriteableBitmap colorBitmap;
        private DepthImagePixel[] depthPixels;
        private byte[] colorPixels;
        public WriteableBitmap view2;

        //depth joints
        public DepthImagePoint left, right, leftShoulder, rightShoulder, head, leftElbow, Elbow;

        //1 means right hand
        //2 means left hand
        int lRhand = 0;

        //for skeleton array
        Skeleton[] skel;

        Point p;
        Point smoothed_hand, smoothed_elbow;

        //objects
        private SwipeGesture worker;
        private Controler cont;

        public MainWindow()
        {
            InitializeComponent();
            progressBar1.Value = 0;
            progressBar1.Maximum = 100;
            progressBar1.Minimum = 0;
            progressBar2.Visibility = Visibility.Hidden;
            Combo.Text = "Gesture";
            Combo.Visibility = System.Windows.Visibility.Collapsed;
            textBlock4.Visibility = Visibility.Hidden;

        }

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            //textBlock1.Text = "5";

            //------main struct



            
            
            if(KinectSensor.KinectSensors.Count > 0)
            {
                //textBlock3.Text = "9";
                
                _sensor = KinectSensor.KinectSensors[0];

                if (_sensor.Status == KinectStatus.Connected)
                {
                    
                   
                    cont = new Controler(this, _sensor);
                    
                    Kinectstatus.Content = "connected";
                    collision = false;
                    _sensor.ColorStream.Enable();
                    _sensor.DepthStream.Enable();
                    _sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(_sensor_AllFramesReady);
                    
                    //for sensor depth
                    _sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);

                    //initializing line point
                    
                    p = new Point(0, 0);
                    smoothed_hand = new Point(0, 0);

                    smoothed_elbow = new Point(0, 0);

                    //sekeleton array
                    this.skel = new Skeleton[6];
                    // Allocate space to put the depth pixels we'll receive
                    this.depthPixels = new DepthImagePixel[_sensor.DepthStream.FramePixelDataLength];

                    // Allocate space to put the color pixels we'll create
                    this.colorPixels = new byte[_sensor.DepthStream.FramePixelDataLength * sizeof(int)];

                    // This is the bitmap we'll display on-screen
                    this.colorBitmap = new WriteableBitmap(_sensor.DepthStream.FrameWidth, _sensor.DepthStream.FrameHeight, 96.0, 96.0, PixelFormats.Bgr32, null);
                    this.view2 = new WriteableBitmap(_sensor.ColorStream.FrameWidth, _sensor.ColorStream.FrameHeight, 96.0, 96.0, PixelFormats.Bgr32, null);
                    this.colorPixels = new byte[_sensor.DepthStream.FramePixelDataLength * sizeof(int)]; 
             

                    // Set the image we display to point to the bitmap where we'll put the image data
                    image1.Source = this.colorBitmap;

                    

                    // Add an event handler to be called whenever there is new depth frame data

                    _sensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(_sensor_DepthFrameReady);

                    var parameters = new TransformSmoothParameters
                    {
                            Smoothing = 0.7f, 
                            Correction = 0.3f, 
                            Prediction = 0.3f, 
                            JitterRadius = 0.4f, 
                            MaxDeviationRadius = 0.5f
                    };

                   
                    _sensor.SkeletonStream.Enable(parameters);

                    _sensor.SkeletonStream.Enable();

                    _sensor.SkeletonStream.AppChoosesSkeletons = false;

                   // worker = new SwipeGesture(this);
                    zaxis = 0.0;
                    
                    _sensor.Start();
                    
                   /*voice = new VoiceRecognition(_sensor, this);
                    voice.init();
                    voice.Start();*/

                    //text = new SpeechToText(this);
                   // text.start();
                    _sensor.ElevationAngle = 10;
                }
            }
        }
     

        KinectSensor _sensor;

        void _sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {


            using (ColorImageFrame colorframe = e.OpenColorImageFrame())
            {

                if (colorframe == null)
                {
                    return;
                }


                byte[] pixels = new byte[colorframe.PixelDataLength];
                colorframe.CopyPixelDataTo(pixels);


                int stride = colorframe.Width * 4;


                this.view2.WritePixels(
                          new Int32Rect(0, 0, this.view2.PixelWidth, this.view2.PixelHeight),
                          pixels,
                          this.view2.PixelWidth * sizeof(int),
                          0);
            }

            Skeleton first = new Skeleton();
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    skel = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    skeletonFrame.CopySkeletonDataTo(skel);

                }



                if (skel.Length != 0)
                {
                    var newNearestId = -1;
                    var nearestDistance2 = double.MaxValue;
                    foreach (Skeleton skeleton in skel)
                    {

                        if (skeleton != null)
                        {
                            

                            if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
                            {

                                // Find the distance squared.
                                var distance2 = (skeleton.Position.X * skeleton.Position.X) +
                                    (skeleton.Position.Y * skeleton.Position.Y) +
                                    (skeleton.Position.Z * skeleton.Position.Z);

                                // Is the new distance squared closer than the nearest so far?
                                if (distance2 < nearestDistance2)
                                {
                                    // Use the new values.
                                    newNearestId = skeleton.TrackingId;
                                    nearestDistance2 = distance2;
                                    first = skeleton;
                                }

                                
                                //getCameraPoint(first, e);
                                //    textBlock2.Text = "got it";
                                //break;
                            }
                        }

                    }
                    
                    if(first.TrackingState == SkeletonTrackingState.Tracked)
                    getCameraPoint(first, e);
                }



            }
        }

        //secondary smoothing paramater
    public double ExponentialMovingAverage(double[] data, double baseValue)
    {
        double numerator = 0;
        double denominator = 0;

        double average = data.Sum();
        average /= data.Length;

        for (int i = 0; i < data.Length; ++i)
        {
            numerator += data[i] * Math.Pow(baseValue, data.Length - i - 1);
            denominator += Math.Pow(baseValue, data.Length - i - 1);
        }

        numerator += average * Math.Pow(baseValue, data.Length);
        denominator += Math.Pow(baseValue, data.Length);

        return numerator / denominator;
    }

    public double WeightedAverage(double[] data, double[] weights)
    {
        if (data.Length != weights.Length)
        {
            return Double.MinValue;
        }

        double weightedAverage = data.Select((t, i) => t * weights[i]).Sum();

        return weightedAverage / weights.Sum();
    }

    private readonly Queue<double> _weightedX = new Queue<double>();
    private readonly Queue<double> _weightedY = new Queue<double>();
    private readonly Queue<double> _weightedZ = new Queue<double>();

    static float ALPHA = 0.15f;

protected float[] lowPass( float[] input, float[] output ) {
    if ( output == null ) return input;

    for ( int i=0; i<input.Length; i++ ) {
        output[i] = output[i] + ALPHA * (input[i] - output[i]);
    }
    return output;
}

   // Point point = ExponentialWeightedAvg(scaledJoint)

    private Point ExponentialWeightedAvg(Joint joint)
    {
        _weightedX.Enqueue(joint.Position.X);
        _weightedY.Enqueue(joint.Position.Y);

        if (_weightedX.Count > 5)
        {
            _weightedX.Dequeue();
            _weightedY.Dequeue();
        }

        
        double x = ExponentialMovingAverage(_weightedX.ToArray(), 0.9);
        double y = ExponentialMovingAverage(_weightedY.ToArray(), 0.9);

        return new Point(x, y);
    }

    
        void getCameraPoint(Skeleton first, AllFramesReadyEventArgs e)
        {

            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {

                if (depth == null || _sensor == null)
                {
                    return;
                }
                right = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(first.Joints[JointType.HandRight].Position, DepthImageFormat.Resolution640x480Fps30);

                left = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(first.Joints[JointType.HandLeft].Position, DepthImageFormat.Resolution640x480Fps30);
                head = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(first.Joints[JointType.Head].Position, DepthImageFormat.Resolution640x480Fps30);
                rightShoulder = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(first.Joints[JointType.ShoulderRight].Position, DepthImageFormat.Resolution640x480Fps30);
                leftShoulder = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(first.Joints[JointType.ShoulderLeft].Position, DepthImageFormat.Resolution640x480Fps30);
                leftElbow = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
                    (first.Joints[JointType.ElbowLeft].Position, DepthImageFormat.Resolution640x480Fps30);
                
                
                ColorImagePoint color_right = _sensor.CoordinateMapper.MapDepthPointToColorPoint(DepthImageFormat.Resolution640x480Fps30, right, ColorImageFormat.RgbResolution1280x960Fps12);
                
                Elbow = _sensor.CoordinateMapper.MapSkeletonPointToDepthPoint
                    (first.Joints[JointType.ElbowRight].Position, DepthImageFormat.Resolution640x480Fps30);
                
                ColorImagePoint Elbow_right = _sensor.CoordinateMapper.MapDepthPointToColorPoint
                 (DepthImageFormat.Resolution640x480Fps30, Elbow, ColorImageFormat.RgbResolution1280x960Fps12);

                ColorImagePoint color_elbow = _sensor.CoordinateMapper.MapDepthPointToColorPoint
                   (DepthImageFormat.Resolution640x480Fps30, Elbow, ColorImageFormat.RgbResolution1280x960Fps12);

               // textBlock6.Text = left.X.ToString();

                /*    
                    if (right.Y < Elbow.Y)
                    {
                        worker.runWorker();
                        worker.setRight_Hand(right.X);
                        worker.setRight_Shoulder(rightShoulder.X);
                        worker.setHead(head.X);
                         worker.setLRhand(1);
                        textBlock4.Text = worker.getRes();
                    }
                    
                if (left.Y < leftElbow.Y) {
                    worker.runWorker();
                    worker.setLeft(left.X);
                    worker.setleft_Shoulder(leftShoulder.X);
                    worker.setHead(head.X);
                    worker.setLRhand(2);
                    textBlock4.Text = worker.getRes();
                }*/
                
                Joint scaledJoint_hand = first.Joints[JointType.HandRight].ScaleTo(1280, 720, .3f, .3f);
             //  Joint scaledJoint_elbow = first.Joints[JointType.ElbowRight].ScaleTo(1280, 720, .3f, .3f);

                smoothed_hand = ExponentialWeightedAvg(scaledJoint_hand);
               // smoothed_elbow = ExponentialWeightedAvg(scaledJoint_elbow);

               //Canvas.SetLeft(icon_hand, color_right.X);
               // Canvas.SetTop(icon_hand, color_right.Y);
                //Canvas.SetLeft(icon_hand, color_right.X - icon_hand.Width / 2);
                //Canvas.SetTop(icon_hand, color_right.Y - icon_hand.Height / 2);

                Canvas.SetLeft(icon_hand, smoothed_hand.X);
                Canvas.SetTop(icon_hand, smoothed_hand.Y );



                //for testing purposes
               // size.Content = smoothed_hand.Y.ToString();
              //  label1.Content = smoothed_hand.X;

                //right hand smoothing
                color_right.X = Convert.ToInt32(smoothed_hand.X);
               
                color_right.Y = Convert.ToInt32(smoothed_hand.Y);
                
                //right elbow smoothing
                //color_elbow.X = Convert.ToInt32(smoothed_elbow.X);
                //color_elbow.Y = Convert.ToInt32(smoothed_elbow.Y);

                
                if (color_right.X < 1138 && (color_right.Y >9 && color_right.Y < 500))
                {
                    
                        cont.Draw(right, color_right, Elbow_right, Elbow);
                   

                }
                else { collision = cont.check(color_right, Elbow_right, right, Elbow); }
                

            }
        }


        void StopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    sensor.Stop();
                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }
                }
            }

        }

        private void Window_Closing(object sender, System.ComponentModel.CancelEventArgs e)
        {
            StopKinect(_sensor);
        }



        //for testing only...
        void _sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            //depth image
            int minDepth, maxDepth, colorPixelIndex;
            using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
            {

                if (depthFrame == null)
                {
                    //textBlock1.Text = "lllll";
                    return;
                }

                // Copy the pixel data from the image to a temporary array
                depthFrame.CopyDepthImagePixelDataTo(this.depthPixels);

                // Get the min and max reliable depth for the current frame
                minDepth = depthFrame.MinDepth;
                maxDepth = depthFrame.MaxDepth;

                // Convert the depth to RGB
                colorPixelIndex = 0;


                for (int i = 0, p = 0; i < this.depthPixels.Length; i++, p += 4)
                {

                    // Get the depth for this pixel
                    short depth = depthPixels[i].Depth;

                    DepthImagePixel depthPixel = this.depthPixels[i];

                    int player1 = depthPixel.PlayerIndex;

                    if (player1 > 0)
                    {
                        // Write out blue byte
                        this.colorPixels[colorPixelIndex++] = 0;

                        // Write out green byte
                        this.colorPixels[colorPixelIndex++] = 215;

                        // Write out red byte                        
                        this.colorPixels[colorPixelIndex++] = 255;

                    }
                    else if (depth <= 900)
                    {
                        // Write out blue byte
                        this.colorPixels[colorPixelIndex++] = 255;

                        // Write out green byte
                        this.colorPixels[colorPixelIndex++] = 0;

                        // Write out red byte                        
                        this.colorPixels[colorPixelIndex++] = 0;
                    }
                    else if (depth > 900 && depth < 2000)
                    {

                        // Write out blue byte
                        this.colorPixels[colorPixelIndex++] = 0;

                        // Write out green byte
                        this.colorPixels[colorPixelIndex++] = 255;

                        // Write out red byte                        
                        this.colorPixels[colorPixelIndex++] = 0;

                    }
                    else if (depth > 2000)
                    {

                        // Write out blue byte
                        this.colorPixels[colorPixelIndex++] = 0;

                        // Write out green byte
                        this.colorPixels[colorPixelIndex++] = 0;

                        // Write out red byte                        
                        this.colorPixels[colorPixelIndex++] = 255;
                    }
                    ++colorPixelIndex;
                }

                // Write the pixel data into our bitmap
                this.colorBitmap.WritePixels(
                    new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight),
                    this.colorPixels,
                    this.colorBitmap.PixelWidth * sizeof(int),
                    0);
            }

        }

       


    }
}
